#include "MPC4.h"
#include "../../ThirdPartyCode/NR_Vector_CVersion.h"
#include "../../ThirdPartyCode/Utilities.h"

MPC4::MPC4( double	iDens_set,
			double	iFact_D, 
			double	iRough2_set,
			double	iFact_R2, 
			double	iHt_set,	
			double	iFact_H, 
			double	it_end,
			double	iDepRate_ini,
			double	iRT_Dep,
			double	it_int_ctrl, 
			int		iStep_num,
			int		iRel_err,
			char*	iFileName,
			int		iDetail,
			int		iLatticeSize,
			int		ControllerTypeID):
MPC_Base( iDens_set,
		  iFact_D,
		  iRough2_set,
		  iFact_R2,
		  iHt_set,
		  iFact_H,
		  it_end,
		  iDepRate_ini,
		  iRT_Dep,
		  it_int_ctrl,
		  iStep_num,
		  iRel_err,
		  iFileName,
		  iDetail,
		  ControllerTypeID)
{
	L = iLatticeSize;
	int_w = 0;

	db_MPC4.open("db_MPC4.txt");
	db_MPC4 << setw(18) << "Estimated SOR" << setw(18) << "Int W";
}
// =============================================
//	This is modified from MPC1::update()
double MPC4::update(struct lattice_state iLState, const double P, const double T)
{	
	int i;
	int	nfunk,paralength=1;
	double *para;
	double ftol=1e-8;
	double dep_ini;
	double cost_min,cost_temp;
	
	// Debug diagnostics
	double dep_ini1,dep_ini3;
	double cost1,cost2,cost3;

//	if(iLState.SOR>1.0 || iLState.SOR < 0.0)	{cerr << "\nError@MPC_Base::update(): illegal value of density. iLState.SOR = " << iLState.SOR << "\n";system("pause");exit(0);}
	if(iLState.R2 < 0.0)						{cerr << "\nError@MPC_Base::update(): illegal value of roughness. iLState.R2 = " << iLState.R2 << "\n";system("pause");exit(0);}
	if(iLState.H < 0.0)							{cerr << "\nError@MPC_Base::update(): illegal value of height. iLState.H = " << iLState.H << "\n";system("pause");exit(0);}
	if(P <= 0.0)								{cerr << "\nError@MPC_Base::update(): illegal value of pressure. P = " << P << "\n";system("pause");exit(0);}
	if(T <= 273)								{cerr << "\nError@MPC_Base::update(): illegal value of temperature. T = " << T << "\n";system("pause");exit(0);}

	alphas	= iLState.alphas;
	betas	= iLState.betas;
	ht		= iLState.H;
	R2	= iLState.R2;

	// State feedback
	//g_dens	= iLState.SOR;
	// Output feedback
	db_g_dens_SOR = iLState.SOR;
	int_w = int_w+iLState.w*t_int_ctrl;
	if(ht != 0)
		g_dens  = int_w/(iLState.top);
	else
		g_dens = 0;

	para		= vector(1,paralength);
	// -------------------------------------------------------------------------------
	upperlimit();
	if(ControllerTypeID == 4) lowerlimit1();	// No height constraints
	else if(ControllerTypeID == 5) lowerlimit2();
	
	for(i=1;i<=Step_num;i++)
	{
		if(lolim[i]>uplim[i])
		{
			cerr << "\nWarning@MPC::update():lolim[" << i << "] > uplim[" << i << "].";
			cerr << "\nlolim[" << i << "] = " << lolim[i];
			//cerr << "\ndep_lower_h = " << dep_lower_h << " ,dep_lower = " << dep_lower << " ,DEP_LOW = " << DEP_LOW;
			cerr << "\nuplim[" << i << "] = " << uplim[i];
			//cerr << "\ndep_upper = " << dep_upper << " ,DEP_HIGH = " << DEP_HIGH;
			cerr << "\nReset lolim[" << i << "] to uplim[" << i << "]-0.001.";
			cerr << "\nConstraints might be unsatisfied\n";
			lolim[i] = uplim[i]-0.001;
		}
	}
	// -------------------------------------------------------------------------------
	// get initial guess using only one prediction step
	// find the minimal cost and the deposition rate for the cost
	para[1]		= 1;
	cost_min	= 1.0e30;
	dep_ini		= dep_rate;
	// 2009.08.06 Xinyu: dep_ini from the old code does not satisfy all the constraints
	for(v_dep_rate[1]=lolim[1];v_dep_rate[1]<=uplim[1];v_dep_rate[1]+=(uplim[1]-lolim[1])/100)
	{
		cost_temp=cost_funct(v_dep_rate,para);
		//printf("W=%f, %f ,",v_dep_rate[1],cost_temp);
		if(cost_temp<cost_min)
		{
			cost_min=cost_temp;
			dep_ini=v_dep_rate[1];
		}
	}
	dep_ini1 = dep_ini;
	cout << "\ndep_ini1 = " << dep_ini1;
	cost1 = cost_min;
	// -------------------------------------------------------------------------------
	// 2008.07.29: deposition rate upper and low bounds, with changing rate constraint
	// para[1]		= 1;			// number of prediction
	/*v_dep_rate[1]	= dep_ini;
	dep_upper		= dep_rate+t_int_ctrl*RT_Dep;
	dep_lower		= dep_rate-t_int_ctrl*RT_Dep;
	uplim1[1]		= DEP_HIGH>dep_upper?dep_upper:DEP_HIGH;
	lolim1[1]		= DEP_LOW>dep_lower?DEP_LOW:dep_lower;
	cost2 = fminsearch(v_dep_rate,ndim,ftol,para,&nffunk,uplim,lolim);
	v_dep_rate2	= v_dep_rate;
	v_dep_rate[1]=uplim[1]>v_dep_rate[1]?v_dep_rate[1]:uplim[1];
	v_dep_rate[1]=lolim[1]>v_dep_rate[1]?lolim[1]:v_dep_rate[1];	// Is the solution of this one step optimization used? (By Xinyu)*/
	//free_vector(lolim,1,1);
	//free_vector(uplim,1,1);
	//free_vector(v_dep_rate,1,1);
	// -------------------------------------------------------------------------------
	/* Model Predictive Control */
	// initialize variales
	para[1]		= Step_num;
	// assignment

	for(i=1;i<=Step_num;i++)
	{
		if(ControllerTypeID == 4)		v_dep_rate[i]	= dep_ini;
		else if(ControllerTypeID == 5)	v_dep_rate[i]	= 0.5*(uplim[i]+lolim[i]);
		// 2008.07.29: adsorption rate upper and low bounds, with changing rate constraint
	}
	cost = fminsearch(v_dep_rate,Step_num,ftol,para,&nfunk,uplim,lolim);
	// the adsorption rate may not exceed bounds
	for(i=1;i<=Step_num;i++)
	{
		v_dep_rate[i]=uplim[i]>v_dep_rate[i]?v_dep_rate[i]:uplim[i];
		v_dep_rate[i]=lolim[i]>v_dep_rate[i]?lolim[i]:v_dep_rate[i];
		/*if(v_dep_rate[i]>uplim[i])
		{
			cerr << "\nERROR@MPC_Base::update():v_dep_rate[" << i << "]>uplim[" << i << "]";
			cerr << "\nv_dep_rate[" << i << "]=" << v_dep_rate[i];
			cerr << "\nuplim[" << i << "]=" << uplim[i];
			cerr << "\nReset v_dep_rate[" << i << "]=uplim[" << i << "]";
			v_dep_rate[i] = uplim[i];
			system("pause");
		}
		else if(v_dep_rate[i] < lolim[i])
		{
			cerr << "\nERROR@MPC_Base::update():v_dep_rate[" << i << "]<lolim[" << i << "]";
			cerr << "\nv_dep_rate[" << i << "]=" << v_dep_rate[i];
			cerr << "\nlolim[" << i << "]=" << lolim[i];
			cerr << "\nReset v_dep_rate[" << i << "]=lolim[" << i << "]";
			v_dep_rate[i] = lolim[i];
			system("pause");
		}*/
	}
	dep_rate = v_dep_rate[1];
	cost = cost_funct(v_dep_rate,para);
	// -------------------------------------------------------------------------------
	if (dep_rate >= 10 || dep_rate <= 0.0) {cerr << "\nError: MPC_Base::update() returns an illegal dep_rate value. dep_rate = " << dep_rate << "\n";system("pause");exit(0);}
	
	time += t_int_ctrl;

	// 2009.08.03 Gangshi: if recording index goes beyond t_node, do not update recording arrays
	if(t_idx<t_node)
	{
		W_sum[t_idx] += dep_rate;
		W2_sum[t_idx] +=  dep_rate*dep_rate;
	}


	if(Detail==1) 
	{
		log_detail();
		db_MPC4 << "\n" << setw(18) << g_dens << setw(18) << int_w;
	}
	t_idx++;

	free_vector(para,1,paralength);

	return dep_rate;


}
// =============================================
void MPC4::lowerlimit1()
// lower bound without height constraints
{
	int i;
	double dep_lower;
	for(i=1;i<=Step_num;i++)
	{
		dep_lower=dep_rate-i*t_int_ctrl*RT_Dep;
		lolim[i] = DEP_LOW>dep_lower?DEP_LOW:dep_lower;
	}
}
// =============================================
void MPC4::lowerlimit2()
// lower bound with height constraints
{
	int i;
	double dep_lower;
	double Rh_lower_h;
	double dep_lower_h;
	
	for(i=1;i<=Step_num;i++)
	{
		// 2008.07.29: adsorption rate upper and low bounds, with changing rate constraint
		dep_lower=dep_rate-i*t_int_ctrl*RT_Dep;
		Rh_lower_h = (Ht_set-ht)/(t_end-time-t_int_ctrl*i);
		dep_lower_h = interp1(TAB_R_RH,TAB_R_DEP_RATE,TAB_R_LENG,Rh_lower_h);
		if(DEP_LOW >= dep_lower && DEP_LOW >= dep_lower_h)
			lolim[i] = DEP_LOW;
		else if(dep_lower>=DEP_LOW && dep_lower >=dep_lower_h)
			lolim[i] = dep_lower;
		else
			lolim[i] = dep_lower_h;
	}
}
// =========================================
void MPC4::reset()
{
	ht		= 0.0;
	R2	= 0.0;
	g_dens	= 0.0;	
	dep_rate	= DepRate_ini;	
	time		= 0.0;
	t_idx		= 0;
	
	W_sum[t_idx] += dep_rate;
	W2_sum[t_idx] +=  dep_rate*dep_rate;

	t_idx ++;
	Repeated += 1;

	int_w = 0;
}


